using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using UnityEngine;

namespace DriverSVH
{
    public class SVHConstants
    {
        // packet sizes
        public const int cPACKET_APPENDIX_SIZE = 8; //!< The packet overhead size in bytes
        public const int cMAIN_PACKET_SIZE = 40;
        public const int cRECIVE_PACKET_SIZE = 72;
        public const int cDEFAULT_PACKET_SIZE = 48; //!< Default packet payload size in bytes

        // packet headers
        public const byte PACKET_HEADER1 = 0x4C;    //!< Header sync byte 1
        public const byte PACKET_HEADER2 = 0xAA;    //!< Header sync byte 2

        // adress constants for commands
        public const byte SVH_GET_CONTROL_FEEDBACK = 0x00; //!< Request the position and current of a channel to be sent
        public const byte SVH_SET_CONTROL_COMMAND = 0x01; //!< Sets the target position of a channel
        public const byte SVH_GET_CONTROL_FEEDBACK_ALL = 0x02; //!< Requests the positions and currents of ALL channels
        public const byte SVH_SET_CONTROL_COMMAND_ALL = 0x03; //!< Sends the target position to ALL the channels
        public const byte SVH_GET_POSITION_SETTINGS = 0x04; //!< Requests the active settings of the position controller
        public const byte SVH_SET_POSITION_SETTINGS = 0x05; //!< Sets new settings for the position controller
        public const byte SVH_GET_CURRENT_SETTINGS = 0x06; //!< Requests the active settings of the current controller
        public const byte SVH_SET_CURRENT_SETTINGS = 0x07; //!< Sets new settings for the current controller
        public const byte SVH_GET_CONTROLLER_STATE = 0x08; //!< Requests the state of the controller (active,faults,enabled channels)
        public const byte SVH_SET_CONTROLLER_STATE = 0x09; //!< Sets new controller states (enable channels, clear faults)
        public const byte SVH_GET_ENCODER_VALUES = 0x0A; //!< Request the current encoder scalings
        public const byte SVH_SET_ENCODER_VALUES = 0x0B; //!< Set new encoder scalings
        public const byte SVH_GET_FIRMWARE_INFO = 0x0C; //!< Request the firmware info to be transmitted

        public static string[] m_channel_description = new string[]{
          "Thumb_Flexion",
          "Thumb_Opposition", // wrist
          "Index_Finger_Distal",
          "Index_Finger_Proximal",
          "Middle_Finger_Distal",
          "Middle_Finger_Proximal",
          "Ring_Finger",
          "Pinky",
          "Finger_Spread",
          null
        };

        public float[] channel_effort_constants = new float[]{
          0.0000232f,  //Thumb Flexion
          0.0000232f,  // Thumb Opposition
          0.000009f,  // Index Finger Distal
          0.000016f,  // Index Finger Proximal
          0.000009f,  // Middle Finger Distal
          0.000016f,  // Middle Finger Proximal
          0.000009f,  // Ring Finger
          0.000009f,  // Pinky
          0.000016f   // Finger Spread
        };

        //! Channel indicates which motor to use in command calls. WARNING: DO NOT CHANGE THE ORDER OF THESE as it represents the hardware mapping
        public enum SVHChannel
        {
            eSVH_ALL = -1,   // this should be used with care as not all functions support it yet
            eSVH_THUMB_FLEXION = 0,
            eSVH_THUMB_OPPOSITION, // wrist
            eSVH_INDEX_FINGER_DISTAL,
            eSVH_INDEX_FINGER_PROXIMAL,
            eSVH_MIDDLE_FINGER_DISTAL,
            eSVH_MIDDLE_FINGER_PROXIMAL,
            eSVH_RING_FINGER,
            eSVH_PINKY,
            eSVH_FINGER_SPREAD,
            eSVH_DIMENSION //9
        };

        /*!
         * \brief The MovementState enum indicated the overall state of the hand. Currently only used for updating the status websocket
         */
        public enum MovementState
        {
            eST_DEACTIVATED,
            eST_RESETTING,
            eST_RESETTED,
            eST_ENABLED,
            eST_PARTIALLY_ENABLED,
            eST_FAULT,
            eST_DIMENSION
        }

        public static byte[] Struct2Bytes<T>(T structure)
        {
            int size = Marshal.SizeOf(typeof(T));
            byte[] bytes = new byte[size];
            IntPtr structPtr = Marshal.AllocHGlobal(size);
            try
            {

                Marshal.StructureToPtr(structure, structPtr, false);
                Marshal.Copy(structPtr, bytes, 0, size);

                return bytes;
            }
            catch (Exception ee)
            {
                Debug.Log(ee);
                return bytes;
            }
            finally
            {
                Marshal.FreeHGlobal(structPtr);
            }
        }

        public static T Bytes2Struct<T>(byte[] bytes)
        {
            object structre = null;
            int size = Marshal.SizeOf(typeof(T));
            IntPtr structPtr = Marshal.AllocHGlobal(size);
            try
            {
                Marshal.Copy(bytes, 0, structPtr, size);
                structre = Marshal.PtrToStructure(structPtr, typeof(T));
            }
            catch (Exception ee)
            {
                Debug.Log(ee);
            }
            finally
            {
                Marshal.FreeHGlobal(structPtr);
            }
            return (T)structre;

        }

    }

    public struct SVHSerialSendPacket
    {
        public byte head1;
        public byte head2;
        public byte index;
        public byte address;
        public UInt16 length; 
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 40)]
        public byte[] data; //(PC -> hand) are always 40 Bytes.
                            //(hand -> PC) are always 64 Bytes
        public byte check_sum1;
        public byte check_sum2;

        public SVHSerialSendPacket(byte add, byte[] da)
        {
            head1 = SVHConstants.PACKET_HEADER1;
            head2 = SVHConstants.PACKET_HEADER2;
            index = 0x00;
            address = add;
            length = 0x28;
            data = da;
            check_sum1 = 0x00;
            check_sum2 = 0x00;
        }
    }

    public struct SVHSerialReceivePacket
    {
        public byte head1;
        public byte head2;
        public byte index;
        public byte address;
        public UInt16 length;
        [MarshalAs(UnmanagedType.ByValArray, SizeConst = 64)]
        public byte[] data; //(PC -> hand) are always 40 Bytes.
                            //(hand -> PC) are always 64 Bytes
        public byte check_sum1;
        public byte check_sum2;
    }
}

